/*
 * @Description: 使用卡尔曼滤波器对坐标滤波
 * @Author: Dryad
 * @Date: 2019-07-05 13:43:04
 */
#pragma once
#include "./Kalman_Filter.hpp"
#include "./Kalman_Filter_Velocity.hpp"

namespace Kalman_Filter_Coor_Namespace
{
union Sys_State_Union {
    struct
    {
        float x;     /* 横向坐标mm */
        float y;     /* 纵向坐标mm */
        float theta; /* 角度坐标rad */
    };               /* 状态量结构体 */
    float data[3];
};

void Init(float dm_x_noise, float dm_y_noise, float dm_angle_noise, /* 输入量为二维码的测量噪声 */
          float ct_x_noise, float ct_angle_noise                    /* 输入量为色带的测量噪声 */
);                                                                  /* 初始化矩阵 */

void Kalman_Filter_Coor(const union Kalman_Filter_Velocity_Namespace::Sys_State_Union &input, /* 输入控制量 */
                        const arm_matrix_instance_f32 &R,                                     /* 控制噪声 */
                        const union Sys_State_Union &measure,                                 /* 二维码测量值 */
                        const float time_s                                                    /* 间隔时间 */
);                                                                                            /* 使用二维码对坐标滤波*/

void Kalman_Filter_Coor(const union Kalman_Filter_Velocity_Namespace::Sys_State_Union &input, /* 输入控制量 */
                        const arm_matrix_instance_f32 &R,                                     /* 控制噪声 */
                        const union Sys_State_Union &measure,                                 /* 色带测量值 */
                        const union Sys_State_Union &coor_base,                               /* 色带基坐标 */
                        const float time_s                                                    /* 间隔时间 */
);                                                                                            /* 使用色带对坐标滤波*/

void Update_Coor_No_Measurement(const union Kalman_Filter_Velocity_Namespace::Sys_State_Union &input, /* 输入控制量 */
                                const arm_matrix_instance_f32 &R,                                     /* 控制噪声 */
                                const float time_s,                                                   /* 间隔时间 */
                                const bool no_measurement_flag = true                                 /* true表示无测量数据，直接更新协方差矩阵 */
);                                                                                                    /* 无外部测量时更新坐标 */

union Sys_State_Union Return_Coor(void); /* 返回状态值 */

void Set_Coor(const union Sys_State_Union &measure, bool is_dm); /* 直接手动设置坐标，is_dm=true表示当前坐标是二维码 */
} // namespace Kalman_Filter_Coor_Namespace
